#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <experimental/filesystem>
#include <iostream>
#include <vector>
#include <fstream>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

namespace fs = std::experimental::filesystem;
using namespace std;
using namespace cv;
// using namespace message_filters;

double timestamp_;
std::string path_save;
int count_ = 0;
bool debug = false;

void savePointCloudAsBin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, const std::string& filename);
void callback(const sensor_msgs::ImageConstPtr &front_left_msg,const sensor_msgs::ImageConstPtr &front_mid_msg,const sensor_msgs::ImageConstPtr &front_right_msg,
                const sensor_msgs::ImageConstPtr &rear_left_msg, const sensor_msgs::ImageConstPtr &rear_right_msg,
                const sensor_msgs::ImageConstPtr &top_left_msg, const sensor_msgs::ImageConstPtr &top_right_msg,
                const sensor_msgs::PointCloud2ConstPtr &pc_left_msg,const sensor_msgs::PointCloud2ConstPtr &pc_right_msg
                );

int main(int argc, char **argv)
{
  path_save = argv[1]; //= "/cv/DataSet/MVS/all_record_raw/q23_seq0_7img2lidar/img7lidar2";
  if (argc > 2)
  {
    debug = true;
  }
  ros::init(argc, argv, "synchronizer");
  ros::NodeHandle nh_;
  message_filters::Subscriber<sensor_msgs::Image> sub_1_;
  message_filters::Subscriber<sensor_msgs::Image> sub_2_;
  message_filters::Subscriber<sensor_msgs::Image> sub_3_;
  message_filters::Subscriber<sensor_msgs::Image> sub_4_;
  message_filters::Subscriber<sensor_msgs::Image> sub_5_;
  message_filters::Subscriber<sensor_msgs::Image> sub_6_;
  message_filters::Subscriber<sensor_msgs::Image> sub_7_;
  message_filters::Subscriber<sensor_msgs::PointCloud2> sub_8_;
  message_filters::Subscriber<sensor_msgs::PointCloud2> sub_9_;
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, 
                      sensor_msgs::Image,sensor_msgs::Image,sensor_msgs::Image,sensor_msgs::PointCloud2,sensor_msgs::PointCloud2> MySyncPolicy;
  typedef message_filters::Synchronizer<MySyncPolicy> Sync;
  boost::shared_ptr<Sync> sync_;
  int count_;

  // syncNode synchronizer;

  count_ = 0;
  sub_1_.subscribe(nh_, "/camera/front_left", 1);
  sub_2_.subscribe(nh_, "/camera/front_mid", 1);
  sub_3_.subscribe(nh_, "/camera/front_right", 1);
  sub_4_.subscribe(nh_, "/camera/rear_left", 1);
  sub_5_.subscribe(nh_, "/camera/rear_right", 1);
  sub_6_.subscribe(nh_, "/camera/top_left", 1);
  sub_7_.subscribe(nh_, "/camera/top_right", 1);
  sub_8_.subscribe(nh_, "/lidar/left", 1);
  sub_9_.subscribe(nh_, "/lidar/right", 1);
  sync_.reset(new Sync(MySyncPolicy(10), sub_1_, sub_2_, sub_3_, sub_4_, sub_5_,sub_6_,sub_7_,sub_8_,sub_9_));
  sync_->registerCallback(
    boost::bind(&callback, _1, _2, _3, _4, _5,_6,_7,_8,_9)
    );
  

  ros::spin();
}


void savePointCloudAsBin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, const std::string& filename)
{
    // Open binary file for writing
    std::ofstream outfile(filename.c_str(), std::ios::out | std::ios::binary);
    // Write point cloud data to binary file
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
        outfile.write((const char*) &cloud->points[i].x, sizeof(float));
        outfile.write((const char*) &cloud->points[i].y, sizeof(float));
        outfile.write((const char*) &cloud->points[i].z, sizeof(float));
        outfile.write((const char*) &cloud->points[i].intensity, sizeof(float));
    }
    // Close binary file
    outfile.close();
}

void callback(const sensor_msgs::ImageConstPtr &front_left_msg,const sensor_msgs::ImageConstPtr &front_mid_msg,const sensor_msgs::ImageConstPtr &front_right_msg,
                const sensor_msgs::ImageConstPtr &rear_left_msg, const sensor_msgs::ImageConstPtr &rear_right_msg,
                const sensor_msgs::ImageConstPtr &top_left_msg, const sensor_msgs::ImageConstPtr &top_right_msg,
                const sensor_msgs::PointCloud2ConstPtr &pc_left_msg,const sensor_msgs::PointCloud2ConstPtr &pc_right_msg
                )
  {
    // if ((count_++ % 8) != 0)
    // {
    //   return;
    // }
    ROS_INFO("Synchronization successful");
    timestamp_ = ros::Time::now().toSec();
    string new_path_save = path_save +"/"+to_string(timestamp_);
    

    if (debug)
    {
      vector<cv::Mat> set1, set2,set3;
      cv::Mat image = cv::Mat::zeros(front_mid_msg->height, front_mid_msg->width, CV_8UC3);
      cv::Mat black = image.clone();
      set1.push_back(black);

      memcpy(image.data, top_left_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
      cv::imwrite(new_path_save + "_top_left.jpg", image);
      cv::Mat tl_img = image.clone();
      set1.push_back(tl_img);

      memcpy(image.data, top_right_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
      cv::imwrite(new_path_save + "_top_right.jpg", image);
      cv::Mat tr_img = image.clone();
      set1.push_back(tr_img);

      // front left
      memcpy(image.data, front_left_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
      cv::imwrite(new_path_save + "_front_left.jpg", image);
      cv::Mat fl_img = image.clone();
      set2.push_back(fl_img);
      
      memcpy(image.data, front_mid_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
      cv::imwrite(new_path_save + "_front_mid.jpg", image);
      cv::Mat fm_img = image.clone();
      set2.push_back(fm_img);

      memcpy(image.data, front_right_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
      cv::imwrite(new_path_save + "_front_right.jpg", image);
      cv::Mat fr_img = image.clone();
      set2.push_back(fr_img);

      memcpy(image.data, rear_left_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
      cv::imwrite(new_path_save + "_rear_left.jpg", image);
      cv::Mat rl_img = image.clone();
      set3.push_back(rl_img);

      set3.push_back(black);

      memcpy(image.data, rear_right_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
      cv::imwrite(new_path_save + "_rear_right.jpg", image);
      cv::Mat rr_img = image.clone();
      set3.push_back(rr_img);


      cv::Mat set1_img,set2_img,set3_img;
      cv::hconcat(set1,set1_img);
      cv::hconcat(set2,set2_img);
      cv::hconcat(set3,set3_img);
      vector<cv::Mat> sets{set1_img,set2_img,set3_img};

      cv::Mat final_img;
      cv::vconcat(sets,final_img);
      // cv::namedWindow("concancate",cv::WINDOW_NORMAL);
      // cv::imshow("concancate", final_img);
      // cv::waitKey(1);
      cv::imwrite(new_path_save + "_final_img.jpg",final_img);

      pcl::PointCloud<pcl::PointXYZI>::Ptr p_l(new pcl::PointCloud<pcl::PointXYZI>);
      pcl::PointCloud<pcl::PointXYZI>::Ptr p_r(new pcl::PointCloud<pcl::PointXYZI>);
      pcl::PCLPointCloud2 pc2_l,pc2_r;
      pcl_conversions::toPCL(*pc_left_msg, pc2_l);
      pcl_conversions::toPCL(*pc_right_msg, pc2_r);
      pcl::fromPCLPointCloud2(pc2_l, *p_l);
      pcl::fromPCLPointCloud2(pc2_r, *p_r);
      savePointCloudAsBin(p_l,new_path_save + "_left_lidar.bin");
      savePointCloudAsBin(p_r,new_path_save + "_right_lidar.bin");
      cout << "show success : " << new_path_save + "_front_left.jpg" << endl;
      exit(0);
    }
    else{
        cv::Mat image = cv::Mat::zeros(front_mid_msg->height, front_mid_msg->width, CV_8UC3);
        // front left
        memcpy(image.data, front_left_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
        cv::imwrite(new_path_save + "_front_left.jpg", image);
        memcpy(image.data, front_right_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
        cv::imwrite(new_path_save + "_front_right.jpg", image);
        memcpy(image.data, rear_left_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
        cv::imwrite(new_path_save + "_rear_left.jpg", image);
        memcpy(image.data, rear_right_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
        cv::imwrite(new_path_save + "_rear_right.jpg", image);
        memcpy(image.data, top_left_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
        cv::imwrite(new_path_save + "_top_left.jpg", image);
        memcpy(image.data, top_right_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
        cv::imwrite(new_path_save + "_top_right.jpg", image);
        memcpy(image.data, front_mid_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
        cv::imwrite(new_path_save + "_front_mid.jpg", image);

        pcl::PointCloud<pcl::PointXYZI>::Ptr p_l(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PointCloud<pcl::PointXYZI>::Ptr p_r(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::PCLPointCloud2 pc2_l,pc2_r;
        pcl_conversions::toPCL(*pc_left_msg, pc2_l);
        pcl_conversions::toPCL(*pc_right_msg, pc2_r);
        pcl::fromPCLPointCloud2(pc2_l, *p_l);
        pcl::fromPCLPointCloud2(pc2_r, *p_r);
        savePointCloudAsBin(p_l,new_path_save + "_left_lidar.bin");
        savePointCloudAsBin(p_r,new_path_save + "_right_lidar.bin");

        cout << "save_path: " << new_path_save + "_front_mid.jpg" << endl;
        // exit(0);
    }

  }
